/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include<global_planner/dijkstra.h>
#include <algorithm>
namespace global_planner {

DijkstraExpansion::DijkstraExpansion(PotentialCalculator* p_calc, int nx, int ny) : Expander(p_calc, nx, ny), pending_(NULL), precise_(false) {
    //初始化优先级 buf 长度为PRIORITYBUFSIZE = 10000
    buffer1_ = new int[PRIORITYBUFSIZE];
    buffer2_ = new int[PRIORITYBUFSIZE];
    buffer3_ = new int[PRIORITYBUFSIZE];

    priorityIncrement_ = 2 * neutral_cost_;
}

DijkstraExpansion::~DijkstraExpansion() {
  delete[] buffer1_;
  delete[] buffer2_;
  delete[] buffer3_;
  if (pending_)
      delete[] pending_;
}

//设置或初始化地图的长度
void DijkstraExpansion::setSize(int xs, int ys) {
    Expander::setSize(xs, ys);
    if (pending_)
        delete[] pending_;
    //初始化未决全部设置为0
    pending_ = new bool[ns_];
    memset(pending_, 0, ns_ * sizeof(bool));
}

//迪杰斯特拉主要的启发函数-广度优先
bool DijkstraExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y, int cycles, float* potential) {
    
    cells_visited_ = 0;                 //设置已经遍历过的栅格为0
    threshold_ = lethal_cost_;          //阈值设置为致命代价
    currentBuffer_ = buffer1_;          //将buffer1的地址传递给当前缓冲区
    currentEnd_ = 0;                    //当前缓冲区的长度设置为0
    nextBuffer_ = buffer2_;             //把第二个缓冲区给下一个缓冲区
    nextEnd_ = 0;
    overBuffer_ = buffer3_;
    overEnd_ = 0;
    memset(pending_, 0, ns_ * sizeof(bool));                //初始化未决全部设置为0
    std::fill(potential, potential + ns_, POT_HIGH);        //初始化 potential里面的值全部为 POT_HIGH = 10e10 10的10次方

    int k = toIndex(start_x, start_y);                      //返回开始点的一维数组下标

    if(precise_)        //判断准确值  使用true新的方式 还是 false:老版navfn启发方式
    {
        double dx = start_x - (int)start_x, dy = start_y - (int)start_y;
        dx = floorf(dx * 100 + 0.5) / 100;                  //向下取整 //保留小数点后两位
        dy = floorf(dy * 100 + 0.5) / 100;
        potential[k] = neutral_cost_ * 2 * dx * dy;         //起始点代价等于  中立代价 *2 * dx * dy  不太明白为啥乘dxdy
        potential[k+1] = neutral_cost_ * 2 * (1-dx)*dy;
        potential[k+nx_] = neutral_cost_*2*dx*(1-dy);
        potential[k+nx_+1] = neutral_cost_*2*(1-dx)*(1-dy);

        push_cur(k+2);      //把k附近的点全部压入 current队列
        push_cur(k-1);
        push_cur(k+nx_-1);
        push_cur(k+nx_+2);

        push_cur(k-nx_);
        push_cur(k-nx_+1);
        push_cur(k+nx_*2);
        push_cur(k+nx_*2+1);
    }else{
        potential[k] = 0;   //当前点的代价设为0
        push_cur(k+1);      //把前后左右全部压入current队列
        push_cur(k-1);
        push_cur(k-nx_);
        push_cur(k+nx_);
    }

    int nwv = 0;            //最大优先级块大小
    int nc = 0;             //放入优先块的信元数量(放进去的栅格的数量)
    int cycle = 0;          // which cycle we're on
    int startCell = toIndex(end_x, end_y);//设置开始单元(分明是关闭好吗)//就是目标点

    for (; cycle < cycles; cycle++){            //进行多次循环，除非被打断,或者循环
        if (currentEnd_ == 0 && nextEnd_ == 0)  //优先权块为空
            return false;

        nc += currentEnd_;                      //统计资料
        if (currentEnd_ > nwv)                  //最大优先级块大小 不能小于 当前队列大小
            nwv = currentEnd_;

        //在当前优先级缓冲区上重置未决标志，把当前缓冲区指针 给pb
        int *pb = currentBuffer_;
        int i = currentEnd_;
        while (i-- > 0)                         //把当前队列的未决全部设置为 false
            pending_[*(pb++)] = false;

        pb = currentBuffer_;                    //处理当前优先级缓冲区
        i = currentEnd_;
        while (i-- > 0)
            updateCell(costs, potential, *pb++);

        currentEnd_ = nextEnd_;                 //交换优先级块currentBuffer_ <=> nextBuffer_
        nextEnd_ = 0;
        pb = currentBuffer_;                    // swap buffers
        currentBuffer_ = nextBuffer_;
        nextBuffer_ = pb;

        if (currentEnd_ == 0) {                 //查看是否已完成此优先级
            threshold_ += priorityIncrement_;   //递增优先级阈值
            currentEnd_ = overEnd_;             //将当前设置为溢出块
            overEnd_ = 0;
            pb = currentBuffer_;                // swap buffers
            currentBuffer_ = overBuffer_;
            overBuffer_ = pb;                   //交换over 和当前队列
        }

        if (potential[startCell] < POT_HIGH)    // check if we've hit the Start cell
            break;
    }
    //ROS_INFO("CYCLES %d/%d ", cycle, cycles);
    if (cycle < cycles)
        return true; // finished up here
    else
        return false;
}

//关键函数: 根据邻居的值计算单元更新后的潜在值
//从四个网格中的两个最低相邻点进行平面更新计算
//内插值的二次近似
//这里没有边界检查,函数应该很快
//二分之根号二
#define INVSQRT2 0.707106781

inline void DijkstraExpansion::updateCell(unsigned char* costs, float* potential, int n) {
    cells_visited_++;                   //已经遍历过的栅格 自加1

    float c = getCost(costs, n);        //做平面更新
    if (c >= lethal_cost_)              //大于致命代价,不要启发到障碍里面去
        return;

    float pot = p_calc_->calculatePotential(potential, c, n);   //pot = 前后左右最小的potential + 当前的costs

    //现在将受影响的邻居添加到优先级块
    if (pot < potential[n]) {
        //获得上下左右的 cost值, 但是不明白为啥要乘以二分之根号二
        float le = INVSQRT2 * (float)getCost(costs, n - 1);
        float re = INVSQRT2 * (float)getCost(costs, n + 1);
        float ue = INVSQRT2 * (float)getCost(costs, n - nx_);
        float de = INVSQRT2 * (float)getCost(costs, n + nx_);
        potential[n] = pot;

        //如果当前 pot小于阈值
        if (pot < threshold_){          //低成本缓冲块,暂时不清楚干啥的
            //如果当前点的潜力  大于 潜力-1 + cost*根号二/2 ，将其push到next的队列
            if (potential[n - 1] > pot + le)
                push_next(n-1);
            if (potential[n + 1] > pot + re)
                push_next(n+1);
            if (potential[n - nx_] > pot + ue)
                push_next(n-nx_);
            if (potential[n + nx_] > pot + de)
                push_next(n+nx_);
        } else            // overflow block
        {
            //如果当前点的潜力  大于 潜力-1 + cost*根号二/2 ，将其push到over的队列
            if (potential[n - 1] > pot + le)
                push_over(n-1);
            if (potential[n + 1] > pot + re)
                push_over(n+1);
            if (potential[n - nx_] > pot + ue)
                push_over(n-nx_);
            if (potential[n + nx_] > pot + de)
                push_over(n+nx_);
        }
    }
}

} //end namespace global_planner
